#include <vector>
#include <cmath>
#include <iostream>
#include <set>

struct Point {
    double x, y;
    Point(double x, double y) : x(x), y(y) {}
};

// 计算两点之间的欧氏距离
double distance(const Point& p1, const Point& p2) {
    return std::sqrt((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y));
}

// 计算三阶贝塞尔曲线上的点
Point cubicBezierPoint(const std::vector<Point>& controlPoints, double t) {
    if (controlPoints.size() != 4) {
        throw std::runtime_error("Three-order Bezier requires exactly 4 control points.");
    }
    double u = 1 - t;
    double b0 = u * u * u;
    double b1 = 3 * u * u * t;
    double b2 = 3 * u * t * t;
    double b3 = t * t * t;

    double x = b0 * controlPoints[0].x +
               b1 * controlPoints[1].x +
               b2 * controlPoints[2].x +
               b3 * controlPoints[3].x;

    double y = b0 * controlPoints[0].y +
               b1 * controlPoints[1].y +
               b2 * controlPoints[2].y +
               b3 * controlPoints[3].y;

    return Point(x, y);
}

// 生成分段三阶贝塞尔曲线，动态调整采样点数量
std::vector<Point> generateCubicBezierPath(const std::vector<Point>& path, double sampleSpacing) {
    std::vector<Point> smoothedPath;
    if (path.size() < 4) return smoothedPath;  // 至少需要 4 个点

    for (size_t i = 0; i + 3 < path.size(); i += 3) {
        // 每段取连续 4 个点作为控制点
        std::vector<Point> controlPoints = {
            path[i],
            path[i + 1],
            path[i + 2],
            path[i + 3]
        };

        // 估算控制点间的路径长度
        double segmentLength = distance(controlPoints[0], controlPoints[1]) +
                               distance(controlPoints[1], controlPoints[2]) +
                               distance(controlPoints[2], controlPoints[3]);

        // 动态计算采样点数量
        int numSamples = std::ceil(segmentLength / sampleSpacing);

        // 遍历 t=[0, 1]，生成采样点
        Point lastPoint = controlPoints[0];
        smoothedPath.push_back(lastPoint);  // 起点
        for (int k = 1; k <= numSamples; ++k) {
            double t = static_cast<double>(k) / numSamples;
            Point newPoint = cubicBezierPoint(controlPoints, t);

            // 去重：如果新点与上一个点距离过近，则跳过
            if (distance(newPoint, lastPoint) > 0.005) {
                smoothedPath.push_back(newPoint);
                lastPoint = newPoint;
            }
        }
    }

    return smoothedPath;
}

int main() {
    // 示例：原始路径
    std::vector<Point> rawPath = {
        Point(0, 0), Point(0.05, 0.1), Point(0.2, 0.2), Point(0.3, 0.1),
        Point(0.4, 0.0), Point(0.45, -0.05), Point(0.5, -0.1), Point(0.6, 0.0)
    };

    // 使用三阶贝塞尔曲线平滑路径，动态调整采样点
    double sampleSpacing = 0.01;  // 采样点间距
    std::vector<Point> smoothedPath = generateCubicBezierPath(rawPath, sampleSpacing);

    // 输出结果
    for (const auto& point : smoothedPath) {
        std::cout << "(" << point.x << ", " << point.y << ")\n";
    }

    return 0;
}
